/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/new_ring_queue.h
 * @Description  : 新环形队列类模板，实现了一个线程安全的环形队列，支持基本的入队、出队、检查队列状态等操作。 
 *                 该队列支持指定大小并可以在满时选择覆盖旧数据或拒绝新数据。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:47:50
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include <mutex>
#include <vector>
#include "z_types.h"
template <typename Type>
class NewRingQueue
{
public:
    /* The constructor */
    explicit NewRingQueue(ushort size) 
        : size_(size), head_pos_(0), tail_pos_(0), count_(0) {
        queue_.resize(size_);
    }

    /* Default constructor */
    NewRingQueue() 
        : size_(0), head_pos_(0), tail_pos_(0), count_(0) {}

    /* The destructor */
    ~NewRingQueue() = default;

    bool Create(ushort size) {
        std::lock_guard<std::mutex> locker(mutex_);
        queue_.resize(size);
        head_pos_ = tail_pos_ = count_ = 0;
        size_ = size;
        return true;
    }

    /*  Test the queue is empty or not */
    bool IsEmpty() {
        std::lock_guard<std::mutex> locker(mutex_);
        return (count_ == 0);
    }

    /*  Test the queue is full or not */
    bool IsFull() {
        std::lock_guard<std::mutex> locker(mutex_);
        return (count_ == size_);
    }

    /* Reset the queue */
    void RemoveAll() {
        std::lock_guard<std::mutex> locker(mutex_);
        head_pos_ = tail_pos_ = count_ = 0;
        queue_.clear();
        queue_.resize(size_);
    }

    /* Put an item into the queue */
    bool Put(const Type& code, bool overwrite = true) {
        if (overwrite) {
            std::lock_guard<std::mutex> locker(mutex_);
            queue_[tail_pos_] = code;
            tail_pos_++;
            if (tail_pos_ == size_) {
                tail_pos_ = 0;
            }
            count_++;
            if (count_ > size_) {
                count_ = size_;
            }
        } else if (IsFull()) {
            return false;
        }
        return true;
    }

    /* Get an item out of the queue */
    Type Get() {
        std::lock_guard<std::mutex> locker(mutex_);
        ushort temp_pos = head_pos_;
        head_pos_++;
        if (head_pos_ == size_) {
            head_pos_ = 0;
        }
        if (count_ > 0) {
            count_--;
        } else {
            count_ = 0;
        }
        return queue_[temp_pos];
    }

    /* Get the count of the items in the queue */
    ushort GetCount() {
        std::lock_guard<std::mutex> locker(mutex_);
        return count_;
    }


    ushort GetSize() {
        std::lock_guard<std::mutex> locker(mutex_);
        return size_;
    }

    /* Retrieve the index of the head entry */
    ushort HeadPos() {
        std::lock_guard<std::mutex> locker(mutex_);
        return head_pos_;
    }

    /* Retrieve the index of the tail entry */
    ushort TailPos() {
        std::lock_guard<std::mutex> locker(mutex_);
        return tail_pos_;
    }

    /* Get the last entry of the specified ushort */
    ushort LastEntry(ushort cur_entry) {
        std::lock_guard<std::mutex> locker(mutex_);
        return (cur_entry == 0) ? size_ - 1 : cur_entry - 1;
    }


    /* Get the next entry of the specified ushort */
    ushort NextEntry(ushort cur_entry) {
        std::lock_guard<std::mutex> locker(mutex_);
        return (cur_entry + 1 == size_) ? 0 : cur_entry + 1;
    }

    /* Proceed the current ushort to the next item */
    bool Proceed(ushort& pos) {
        std::lock_guard<std::mutex> locker(mutex_);
        ushort next_pos = (pos + 1 == size_) ? 0 : pos + 1;
        if (IsAnItem(pos)) {
            pos = next_pos;
            return true;
        }
        return false;
    }

    /* Retrieve the specified item (if not a member of the queue, return NULL) */
    Type PeekItem(ushort pos_item) {
        std::lock_guard<std::mutex> locker(mutex_);
        return IsAnItem(pos_item) ? queue_[pos_item] : Type();
    }


    /* Add data to the ring buffer */
    bool AddData(Type code) {
        std::lock_guard<std::mutex> locker(mutex_);
        queue_[tail_pos_] = code;
        tail_pos_++;
        if (tail_pos_ == size_) {
            tail_pos_ = 0;
        }
        count_++;
        if (count_ > size_) {
            count_ = size_;
        }
        return true;
    }

private:
    /* Check whether the specified ushort is an item in the queue */
    bool IsAnItem(ushort pos) {
        if (count_ == 0) {
            return false;
        }
        if (pos < 0 || pos >= size_) {
            return false;
        }
        if (tail_pos_ == head_pos_) {
            return (pos < size_);
        } else if (tail_pos_ > head_pos_) {
            return (pos >= head_pos_ && pos < tail_pos_);
        } else {
            return (pos < tail_pos_ || pos >= head_pos_);
        }
    }

    std::mutex mutex_;
    std::vector<Type> queue_;  // The buffer for the queue
    ushort size_;               // Size of the ring buffer
    ushort head_pos_;           // The ushort of the head entry
    ushort tail_pos_;           // The ushort of the tail entry
    ushort count_;              // The count of items
};

